#ifndef TC_H
#define TC_H
#include "tp_type.h"

int tcLine9Init(STRUCT_PM_LINE9 *line9, STRUCT_POSE *start, STRUCT_POSE *end, int ik_flag);
int tcGetLine9TargetLen(STRUCT_PM_LINE9 *line9, double *target);
int tcGetLine9Pose(STRUCT_PM_LINE9 *line9, STRUCT_POSE *pose, double progress, double target);
int tcLine9SelfSub(STRUCT_POSE *p1, STRUCT_POSE *p2);
int tcLine9SelfAdd(STRUCT_POSE *p1, STRUCT_POSE *p2);

//------------------------------
int tcP2PGetPosit(STRUCT_TC *tc, double posit[MAX_JOINT]);
int tcP2PSelfSub(double p1[MAX_JOINT], double p2[MAX_JOINT]);
int tcP2PSelfAdd(double p1[MAX_JOINT], double p2[MAX_JOINT]);


//-------------------------------
int tcGetEndTangentUnitVector(STRUCT_TC *tc, STRUCT_PM_CARTESIAN *out);
int tcGetStartTangentUnitVector(STRUCT_TC *tc, STRUCT_PM_CARTESIAN *out);

//-------------------------------
int tcSetSplitCycle(STRUCT_TC *tc, double split_time, double v_f);
int tcIsBlending(STRUCT_TC *tc);


//-------------------------------
int tcInitSegment(STRUCT_TC *tc, STRUCT_USER_VEL user_vel, STRUCT_USER_OVL user_ovl, int Id, char enable);
int tcActiveSegment(STRUCT_TC *tc);
int tcCheckUserVelAndOvl(STRUCT_TC *tc);
int tcPreProcessSegment_P2P(STRUCT_TC *tc);
int tcPreProcessSegment_Line(STRUCT_TC *tc);


//-------------------------------
void tcPose2Cartesian(STRUCT_POSE *pose, STRUCT_PM_CARTESIAN *xyz, STRUCT_PM_CARTESIAN *rxyz, STRUCT_PM_CARTESIAN *uvw);
int tcCartCartSub(STRUCT_PM_CARTESIAN *v1, STRUCT_PM_CARTESIAN *v2, STRUCT_PM_CARTESIAN *vout);
int tcCartesianLineInit(STRUCT_CART_LINE *line, STRUCT_PM_CARTESIAN *start, STRUCT_PM_CARTESIAN *end);
void tcCartMagnitude(STRUCT_PM_CARTESIAN *vec, double *d);
int tcCartLinePoint(STRUCT_CART_LINE *line, STRUCT_PM_CARTESIAN *point, double progress);
int tcCartCartDot(STRUCT_PM_CARTESIAN *v1, STRUCT_PM_CARTESIAN *v2, double *d);
int tcCartScalMult(STRUCT_PM_CARTESIAN *v1, double d, STRUCT_PM_CARTESIAN *vout);


#endif
